#!/bin/bash

# 设置颜色代码
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
RED='\033[0;31m'
NC='\033[0m' # No Color

# 设置ROS环境
source /opt/ros/noetic/setup.bash
source /root/catkin_ws/devel/setup.bash

# 默认参数
MAP_FILE="/root/catkin_ws/src/auto_navigation/maps/kitti_best_map.pcd"
USE_IMU=false
USE_ODOM=false
USE_GNSS=false
BAG_FILE=""
BAG_RATE=1.0
BAG_START=0.0

# 显示帮助信息
function show_help {
    echo -e "${BLUE}=== NDT定位运行脚本 ===${NC}"
    echo "用法: $0 [选项]"
    echo ""
    echo "选项:"
    echo "  -h, --help                显示帮助信息"
    echo "  -m, --map FILE            指定地图文件路径 (默认: $MAP_FILE)"
    echo "  -i, --imu                 使用IMU数据"
    echo "  -o, --odom                使用里程计数据"
    echo "  -g, --gnss                使用GNSS数据"
    echo "  -b, --bag FILE            指定bag文件路径"
    echo "  -r, --rate RATE           指定bag文件回放速率 (默认: 1.0)"
    echo "  -s, --start TIME          指定bag文件起始时间 (默认: 0.0)"
    echo ""
    echo "示例:"
    echo "  $0 -m /path/to/map.pcd -i -o -b /path/to/data.bag -r 0.5"
    echo ""
}

# 解析命令行参数
while [[ $# -gt 0 ]]; do
    key="$1"
    case $key in
        -h|--help)
            show_help
            exit 0
            ;;
        -m|--map)
            MAP_FILE="$2"
            shift
            shift
            ;;
        -i|--imu)
            USE_IMU=true
            shift
            ;;
        -o|--odom)
            USE_ODOM=true
            shift
            ;;
        -g|--gnss)
            USE_GNSS=true
            shift
            ;;
        -b|--bag)
            BAG_FILE="$2"
            shift
            shift
            ;;
        -r|--rate)
            BAG_RATE="$2"
            shift
            shift
            ;;
        -s|--start)
            BAG_START="$2"
            shift
            shift
            ;;
        *)
            echo -e "${RED}错误: 未知选项 $1${NC}"
            show_help
            exit 1
            ;;
    esac
done

# 检查地图文件是否存在
if [ ! -f "$MAP_FILE" ]; then
    echo -e "${RED}错误: 地图文件不存在: $MAP_FILE${NC}"
    exit 1
fi

# 显示配置信息
echo -e "${BLUE}=== NDT定位配置 ===${NC}"
echo -e "地图文件: ${YELLOW}$MAP_FILE${NC}"
echo -e "使用IMU: ${YELLOW}$USE_IMU${NC}"
echo -e "使用里程计: ${YELLOW}$USE_ODOM${NC}"
echo -e "使用GNSS: ${YELLOW}$USE_GNSS${NC}"

# 构建roslaunch命令
LAUNCH_CMD="roslaunch auto_navigation ndt_localization.launch"
LAUNCH_CMD="$LAUNCH_CMD map_file:=$MAP_FILE"
LAUNCH_CMD="$LAUNCH_CMD use_imu:=$USE_IMU"
LAUNCH_CMD="$LAUNCH_CMD use_odom:=$USE_ODOM"
LAUNCH_CMD="$LAUNCH_CMD use_gnss:=$USE_GNSS"

# 如果指定了bag文件，启动rosbag play
if [ -n "$BAG_FILE" ]; then
    if [ ! -f "$BAG_FILE" ]; then
        echo -e "${RED}错误: bag文件不存在: $BAG_FILE${NC}"
        exit 1
    fi
    
    echo -e "bag文件: ${YELLOW}$BAG_FILE${NC}"
    echo -e "回放速率: ${YELLOW}$BAG_RATE${NC}"
    echo -e "起始时间: ${YELLOW}$BAG_START${NC}"
    
    # 在后台启动roslaunch
    echo -e "${GREEN}启动NDT定位...${NC}"
    $LAUNCH_CMD &
    LAUNCH_PID=$!
    
    # 等待几秒钟，确保节点已启动
    echo -e "${YELLOW}等待节点启动...${NC}"
    sleep 3
    
    # 启动rosbag play
    echo -e "${GREEN}开始回放bag文件...${NC}"
    rosbag play --clock $BAG_FILE -r $BAG_RATE -s $BAG_START
    
    # 回放结束后，终止roslaunch进程
    echo -e "${YELLOW}回放结束，终止NDT定位...${NC}"
    kill $LAUNCH_PID
else
    # 直接启动roslaunch
    echo -e "${GREEN}启动NDT定位...${NC}"
    $LAUNCH_CMD
fi

echo -e "${BLUE}=== NDT定位已完成 ===${NC}" 